/*
 * ThreadMotorController.cpp
 *
 *  Created on: Jan 26, 2013
 *      Author: Drew Prevost
 */

#include "ThreadMotorController.h"

ThreadMotorController::ThreadMotorController( int nPin, int nAccPerSecond )
{
  this->nPin = nPin;
  this->nCurSpeed = 0;
  this->nTargetSpeed = 0;
  this->nAccPerSecond = nAccPerSecond;
  this->nDiff = 0;
  this->bDiffIsZero = true;
  this->nAccDirection = 0;
  this->nNextMotorSpeed = 0;
}

ThreadMotorController::~ThreadMotorController()
{
}

int ThreadMotorController::getSpeed()
{
  return nCurSpeed;
}

void ThreadMotorController::setSpeed( int nTargetSpeed, int nAccPerSecond )
{
  this->nTargetSpeed = nTargetSpeed;
  this->nAccPerSecond = nAccPerSecond;
}

//// Local variables problematic in protothreads so I made this macro to compute next position.
//#define __MOTOR_SPEED_NEXT(nDiffFromTarget) (nCurSpeed) + ( ((nDiffFromTarget) > 0) ? 1 : -1 )
//#define __MOTOR_DELAY_MICROS() ( ( (nAccRateInValPerSec) > 0 ) ? 1000000 / (nAccRateInValPerSec) : 0 )

//bool ThreadMotorController::run()
//{
//  threadMicroDelay.run();
//
//  int nDiff = (int) nTargetSpeed - (int) nCurSpeed;
//
//  PT_BEGIN()
//  ;
//
//  if ( nDiff != 0 )
//  {
//    // tell motor to move 1 value to next speed
//    analogWrite( nPin, __MOTOR_SPEED_NEXT( nDiff) );
//
//    // update current speed in class
//    nCurSpeed = __MOTOR_SPEED_NEXT( nDiff);
//
//    // delay some to implement the move rate.  So this is (1000000 / nAccRateInValPerSec)
//    if ( ( nAccRateInValPerSec > 0 ) && ( nTargetSpeed != nCurSpeed ) )
//    {
//      threadMicroDelay.setDelay( __MOTOR_DELAY_MICROS() );
//      PT_SPAWN( threadMicroDelay );
//    }
//  }
//
//PT_END()
//;
//return false;
//}

//bool ThreadMotorController::run()
//{
//  threadMicroDelay.run();
//
//  int nDiff = (int) nTargetSpeed - (int) nCurSpeed;
//
//  bool bDiffIsZero = ( ( nDiff == 0 ) ? true : false );
//
//  if ( !bDiffIsZero )
//  {
//    int nAccDirection = ( ( nDiff > 0 ) ? 1 : -1 );
//
//    int nNextMotorSpeed = ( nCurSpeed + nAccDirection );
//
//    analogWrite( nPin, nNextMotorSpeed );
//
//    nCurSpeed = nNextMotorSpeed;
//
////    static char szMsg[128] = {};
//
//    PT_BEGIN();
//
//
//    Serial.println( " ThreadMotorController::run() begin " );
//
//
////    sprintf(szMsg, "Before Delay: micros = %lu, nAccRateInValPerSec = %lu", micros(), nAccRateInValPerSec );
////    Serial.println( szMsg );
//
//    threadMicroDelay.setDelay( nAccRateInValPerSec );  // NOTE TO SELF:  this is actually micros per acc for the time being
//    PT_SPAWN(threadMicroDelay);
//
////    sprintf(szMsg, "After Delay: micros = %lu, nAccRateInValPerSec = %lu", micros(), nAccRateInValPerSec );
////    Serial.println( szMsg );
//
//
//    Serial.println( " ThreadMotorController::run() complete " );
//
//    PT_END();
//    return false;
//  }
//}

bool ThreadMotorController::run()
{
  threadMicroDelay.run();

  PT_BEGIN()
  ;

  nDiff = (int) nTargetSpeed - (int) nCurSpeed;

  bDiffIsZero = ( ( nDiff == 0 ) ? true : false );

  if ( !bDiffIsZero )
  {
    nAccDirection = ( ( nDiff > 0 ) ? 1 : -1 );

    nNextMotorSpeed = ( nCurSpeed + nAccDirection );

    analogWrite( nPin, nNextMotorSpeed );

    nCurSpeed = nNextMotorSpeed;

//    Serial.println( nCurSpeed );

    threadMicroDelay.setDelay( nAccPerSecond );
    PT_SPAWN( threadMicroDelay );
  }

PT_END()
;
return false;
}

